/*
 *  Quadtree.cpp
 *  
 *
 *  Created by Daniel Condon on 24/08/12.
 *  Copyright 2012 __MyCompanyName__. All rights reserved.
 *
 */

#include "Quadtree.h"
#include <iostream>
using namespace std;
Quadtree::Quadtree( float nx, float ny, float nwidth, float nheight, int ndepth, int nmaxDepth ){
	x = nx;
	y = ny;
	width = nwidth;
	height = nheight;
	depth = ndepth;
	maxDepth = nmaxDepth;
	
	if ( depth < maxDepth ) {
		NW = new Quadtree(x - width / 2.0f, y + height / 2.0f, width / 2.0f, height / 2.0f, depth+1, maxDepth);
		NE = new Quadtree(x + width / 2.0f, y + height / 2.0f, width / 2.0f, height / 2.0f, depth+1, maxDepth);
		SW = new Quadtree(x - width / 2.0f, y - height / 2.0f, width / 2.0f, height / 2.0f, depth+1, maxDepth);
		SE = new Quadtree(x + width / 2.0f, y - height / 2.0f, width / 2.0f, height / 2.0f, depth+1, maxDepth);		
	}
}

Quadtree::~Quadtree(){
	if (depth < maxDepth) {
		delete NW;
		delete NE;
		delete SW;
		delete SE;
	}
	objects.erase(objects.begin(), objects.end());
}

Vec Quadtree::getPos(){
	return Vec(x,y);
}

Vec Quadtree::getPoint(int point){
	Vec ret;
	switch(point){
		case 1:
			ret.setX(x-width);
			ret.setY(y+height);
			break;
		case 2:
			ret.setX(x+width);
			ret.setY(y+height);
			break;
		case 3:
			ret.setX(x+width);
			ret.setY(y-height);
			break;
		case 4:
			ret.setX(x-width);
			ret.setY(y-height);
			break;
	}
	return ret;
}
Brain* Quadtree::getCreature(int which){
	return objects.at(which);
}

void Quadtree::AddCreature( Brain *object ) {
	if ( depth == maxDepth ) {
		objects.push_back( object );
		return;
	}
	if ( Contains( NW, object ) ) {
		NW->AddCreature( object ); return;
	} else if ( Contains( NE, object ) ) {
		NE->AddCreature( object ); return;
	} else if ( Contains( SW, object ) ) {
		SW->AddCreature( object ); return;
	} else if ( Contains( SE, object ) ) {
		SE->AddCreature( object ); return;
	}
	if ( Contains( this, object ) ) {
		objects.push_back( object );
	}
}


//will return all creatures in leaf
vector<Brain*> Quadtree::GetCreaturesAt( double vx, double vy ) {
	if ( depth == maxDepth ) {
		return objects;
	}
	
	vector<Brain*> returnBrains, childReturnBrains;
	if ( !objects.empty() ) {
		returnBrains = objects;
	}
	
	if (vy > y && vy < y + height) {
		//test for N > y
		if (vx < x && vx > x - width) {
			//test for NW < x
			childReturnBrains = NW->GetCreaturesAt( vx, vy );
			returnBrains.insert( returnBrains.end(), childReturnBrains.begin(), childReturnBrains.end() );
			return returnBrains;
		}else if(vx > x && vx < x + width){
			//test for NE > x
			childReturnBrains = NE->GetCreaturesAt( vx, vy );
			returnBrains.insert( returnBrains.end(), childReturnBrains.begin(), childReturnBrains.end() );
			return returnBrains;
		}
	}else if (vy < y && vy > y - height) {
		//test for S < y
		if (vx < x && vx > x - width) {
			//test for SW < x
			childReturnBrains = SW->GetCreaturesAt( vx, vy );
			returnBrains.insert( returnBrains.end(), childReturnBrains.begin(), childReturnBrains.end() );
			return returnBrains;
		}else if (vx > x && vx < x + width) {
			//test for SE > x
			childReturnBrains = SE->GetCreaturesAt( vx, vy );
			returnBrains.insert( returnBrains.end(), childReturnBrains.begin(), childReturnBrains.end() );
			return returnBrains;
		}
	}
	return returnBrains;
}

vector<Brain*> Quadtree::GetCreaturesNear(double vx, double vy, double dist){
	vector<Brain*> CreaturesInRadius, children;
	if (!objects.empty()) {
		for (int i = 0; i < objects.size(); i++) {
			if (objects.at(i)->getPos().Dist(Vec(vx, vy)) <= dist) {
				CreaturesInRadius.push_back(objects.at(i));
			}
		}
	}else if (depth == maxDepth) {
		return CreaturesInRadius;
	}
	if (depth < maxDepth) {
		if (InRange(NE, Vec(vx, vy), dist)) {
			children = NE->GetCreaturesNear(vx, vy, dist);
			CreaturesInRadius.insert(CreaturesInRadius.end(), children.begin(), children.end());
		}
		if (InRange(NW, Vec(vx, vy), dist)) {
			children = NW->GetCreaturesNear(vx, vy, dist);
			CreaturesInRadius.insert(CreaturesInRadius.end(), children.begin(), children.end());
		}
		if (InRange(SW, Vec(vx, vy), dist)) {
			children = SW->GetCreaturesNear(vx, vy, dist);
			CreaturesInRadius.insert(CreaturesInRadius.end(), children.begin(), children.end());
		}
		if (InRange(SE, Vec(vx, vy), dist)) {
			children = SE->GetCreaturesNear(vx, vy, dist);
			CreaturesInRadius.insert(CreaturesInRadius.end(), children.begin(), children.end());
		}
	}
	return CreaturesInRadius;
}

int Quadtree::getSize(){
	return objects.size();
}

void Quadtree::Clear() {
	if ( depth == maxDepth ) {
		objects.clear();
		return;
	} else {
		NW->Clear();
		NE->Clear();
		SW->Clear();
		SE->Clear();
	}
	if ( !objects.empty() ) {
		objects.clear();
	}
}

bool Quadtree::Contains( Quadtree *child, Brain *object ) {
	//test if object is in bounds
	//objX+size < x+w
	//objX-size > x-w
	//objY+size < y+h
	//objY-size > y-h
	
	if(object->getPos().getX() + object->getSize() < child->x + child->width &&
	   object->getPos().getX() - object->getSize() > child->x - child->width)
	{
		if (object->getPos().getY() + object->getSize() < child->y + child->height && 
			object->getPos().getY() - object->getSize() > child->y - child->height) {
			//whole body is in
			return true;
		}
	}
	return false;
}

bool Quadtree::InRange( Quadtree *child, Vec point, double range) {
	if (child->getPos().Dist(point) <= range) {
		return true;
	}else {
		bool up, down, left, right;
		if (point.getY() < child->getPos().getY()) {
			down = true;
		}else {
			up = true;
		}
		if (point.getX() < child->getPos().getX()) {
			left = true;
		}else {
			right = true;
		}
		
		if (left) {
			if (up) {
				if (child->getPoint(1).Dist(point) <= range) {
					return true;
				}
			}else if (down) {
				if (child->getPoint(4).Dist(point) <= range) {
					return true;
				}
			}
		}else if (right) {
			if (up) {
				if (child->getPoint(2).Dist(point) <= range) {
					return true;
				}
			}else if (down) {
				if (child->getPoint(3).Dist(point) <= range) {
					return true;
				}
			}
		}
	}
	return false;
}


//DEBUG
vector<Vec> Quadtree::points(){
	vector<Vec> ret, childRet;
	if (!objects.empty()) {
		ret.push_back(Vec(x-width,y+height));
		ret.push_back(Vec(x+width,y-height));
		ret.push_back(Vec(x+width,y-height));
		ret.push_back(Vec(x-width,y+height));
	}
	
	if (depth < maxDepth) {
		childRet = NW->points();
		ret.insert(ret.end(), childRet.begin(), childRet.end());
		
		childRet = NE->points();
		ret.insert(ret.end(), childRet.begin(), childRet.end());
		
		childRet = SW->points();
		ret.insert(ret.end(), childRet.begin(), childRet.end());
		
		childRet = SE->points();
		ret.insert(ret.end(), childRet.begin(), childRet.end());
	}
	return ret;
}